#include <SPI.h>
#include <Wire.h>
#include <TimerOne.h>
#include "nRF24L01.h"
#include "RF24.h"
#include "printf.h"
#include "queue.h"
#include "MadgwickAHRS.h"

RF24 radio(9,10);
const uint64_t pipe=0x2331636685LL;
int tiltInput1=2;
int tiltInput2=4;

#define    EPS                        1e-3

#define    MPU9250_ADDRESS            0x68
#define    MAG_ADDRESS                0x0C

#define    GYRO_FULL_SCALE_250_DPS    0x00  
#define    GYRO_FULL_SCALE_500_DPS    0x08
#define    GYRO_FULL_SCALE_1000_DPS   0x10
#define    GYRO_FULL_SCALE_2000_DPS   0x18

#define    ACC_FULL_SCALE_2_G        0x00  
#define    ACC_FULL_SCALE_4_G        0x08
#define    ACC_FULL_SCALE_8_G        0x10
#define    ACC_FULL_SCALE_16_G       0x18

#define RATIO_ACC         (4./32767.)
#define RATIO_GYRO        ((1000./32767.)*(M_PI/180.))
#define RATIO_MAG         (48.0 / 32767.0)

#define MAX_GYRO    1.065

#define sampleFreq  512.0f    // sample frequency in Hz
#define betaDef       0.1f    // 2 * proportional gain

float wx, wy, wz;
int dwx, dwy, dwz;

volatile float beta = betaDef;
volatile float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;

Queue q_phi, q_theta, q_psi;

//---------------------------------------------------------------------------------------------------
// Function declarations

float invSqrt(float x);


// This function read Nbytes bytes from I2C device at address Address. 
// Put read bytes starting at register Register in the Data array. 
void I2Cread(uint8_t Address, uint8_t Register, uint8_t Nbytes, uint8_t* Data)
{
  // Set register address
  Wire.beginTransmission(Address);
  Wire.write(Register);
  Wire.endTransmission();
  
  // Read Nbytes
  Wire.requestFrom(Address, Nbytes); 
  uint8_t index=0;
  while (Wire.available())
    Data[index++]=Wire.read();
}


// Write a byte (Data) in device (Address) at register (Register)
void I2CwriteByte(uint8_t Address, uint8_t Register, uint8_t Data)
{
  // Set register address
  Wire.beginTransmission(Address);
  Wire.write(Register);
  Wire.write(Data);
  Wire.endTransmission();
}



// Initial time
unsigned long ti;
volatile bool intFlag=false;

// Initializations
void setup()
{
  // Arduino initializations
  Wire.begin();
  Serial.begin(115200);

  radio.begin();
  radio.openWritingPipe(pipe);
  radio.openReadingPipe(0,pipe);
  radio.setChannel(40);
  radio.setPayloadSize(32);
  radio.setDataRate(RF24_2MBPS);
  radio.setCRCLength(RF24_CRC_16);
  printf_begin();
  radio.printDetails();
  pinMode(tiltInput1,INPUT);
  pinMode(tiltInput2,INPUT);
  
  // Set accelerometers low pass filter at 5Hz
  I2CwriteByte(MPU9250_ADDRESS,29,0x06);
  // Set gyroscope low pass filter at 5Hz
  I2CwriteByte(MPU9250_ADDRESS,26,0x06);
 
  
  // Configure gyroscope range
  I2CwriteByte(MPU9250_ADDRESS,27,GYRO_FULL_SCALE_1000_DPS);
  // Configure accelerometers range
  I2CwriteByte(MPU9250_ADDRESS,28,ACC_FULL_SCALE_4_G);
  // Set by pass mode for the magnetometers
  I2CwriteByte(MPU9250_ADDRESS,0x37,0x02);
  
  // Request continuous magnetometer measurements in 16 bits
  I2CwriteByte(MAG_ADDRESS,0x0A,0x16);
  
//  pinMode(13, OUTPUT);
  Timer1.initialize(10000);         // initialize timer1, and set a 1/2 second period
  Timer1.attachInterrupt(callback);  // attaches callback() as a timer overflow interrupt

//  q_phi.clear();
//  q_theta.clear();
//  q_psi.clear();
  wx = wy = wz = 0.0;
  dwx = dwy = dwz = 0;
  
  // Store initial time
  ti=millis();
}

int TS=0,nowTS=0;
int count=0;
int upSignal=0;

// Counter
unsigned long cpt=0;

void callback()
{ 
  intFlag=true;
  digitalWrite(13, digitalRead(13) ^ 1);
}


// Main loop, read and display data
void loop()
{
  while (!intFlag);
  intFlag=false;
  
  // Display time
  unsigned long curTime = millis() - ti;
  Serial.print (curTime, DEC);
  Serial.print ("\t");

  
  // _______________
  // ::: Counter :::
  
  // Display data counter
//  Serial.print (cpt++,DEC);
//  Serial.print ("\t");
 
  // ____________________________________
  // :::  accelerometer and gyroscope ::: 

  // Read accelerometer and gyroscope
  uint8_t Buf[14];
  I2Cread(MPU9250_ADDRESS,0x3B,14,Buf);
  
  // Create 16 bits values from 8 bits data
  
  // Accelerometer
  int16_t iax = -(Buf[0]<<8 | Buf[1]);
  int16_t iay = -(Buf[2]<<8 | Buf[3]);
  int16_t iaz =   Buf[4]<<8 | Buf[5];

  // Gyroscope
  int16_t igx = -(Buf[8] <<8 | Buf[9]);
  int16_t igy = -(Buf[10]<<8 | Buf[11]);
  int16_t igz =   Buf[12]<<8 | Buf[13];
  
    // Display values
  
  // Accelerometer
//  Serial.print (iax,DEC); 
//  Serial.print ("\t");
//  Serial.print (iay,DEC);
//  Serial.print ("\t");
//  Serial.print (iaz,DEC);  
//  Serial.print ("\t");
  
  // Gyroscope
  Serial.print (igx,DEC); 
  Serial.print ("\t");
  Serial.print (igy,DEC);
  Serial.print ("\t");
  Serial.print (igz,DEC);  
  Serial.print ("\t");

  
  // _____________________
  // :::  Magnetometer ::: 

  
  // Read register Status 1 and wait for the DRDY: Data Ready
  
  uint8_t ST1;
  do
  {
    I2Cread(MAG_ADDRESS,0x02,1,&ST1);
  }
  while (!(ST1&0x01));

  // Read magnetometer data  
  uint8_t Mag[7];  
  I2Cread(MAG_ADDRESS,0x03,7,Mag);
  

  // Create 16 bits values from 8 bits data
  
  // Magnetometer
  int16_t imx = -(Mag[3]<<8 | Mag[2]);
  int16_t imy = -(Mag[1]<<8 | Mag[0]);
  int16_t imz = -(Mag[5]<<8 | Mag[4]);
  
  imx += 200;
  imy += -70;
  imz += -700;
  
  // Magnetometer
//  Serial.print (imx,DEC); 
//  Serial.print ("\t");
//  Serial.print (imy,DEC);
//  Serial.print ("\t");
//  Serial.print (imz,DEC);  
//  Serial.print ("\t");

  float ax, ay, az;
  float gx, gy, gz;
  float mx, my, mz;

  ax=iax*RATIO_ACC;
  ay=iay*RATIO_ACC;
  az=iaz*RATIO_ACC;
  
  gx= igx * RATIO_GYRO; // (igx-48.4827)*RATIO_GYRO;
  gy= igy * RATIO_GYRO; // (igy+76.3552)*RATIO_GYRO;
  gz= igz * RATIO_GYRO; // (igz+64.3234)*RATIO_GYRO;
  
  mx=imx*RATIO_MAG;
  my=imy*RATIO_MAG;
  mz=imz*RATIO_MAG;

  Serial.print(gx);
  Serial.print("\t");
  Serial.print(gy);
  Serial.print("\t");

  if (gx < EPS && gx > -EPS) gx = 0.0;
  if (gy < EPS && gy > -EPS) gy = 0.0;
  if (gz < EPS && gz > -EPS) gz = 0.0;

  wx = 0.80 * (wx + 0.3 * gx);
  wy = 0.80 * (wy + 0.3 * gy);
  wz = 0.80 * (wz + 0.3 * gz);

  Serial.print(wx);
  Serial.print("\t");
  Serial.print(wy);
  Serial.print("\t");
  Serial.print(wz);
  Serial.print("\t");
  
  if (wx > 10.0) {
    wx = 10.0;
  } else if (wx < -10.0) {
    wx = -10.0;
  }
  
  if (wy > 10.0) {
    wy = 10.0;
  } else if(wy < -10.0) {
    wy = -10.0;
  }
  
  if (wz > 10.0) {
    wz = 10.0;
  } else if (wz < -10.0) {
    wz = -10.0;
  }
  
  dwx = int(wx)+10;
  dwy = int(wy)+10;
  dwz = int(wz)+10;
  Serial.print(dwx, DEC);
  Serial.print("\t");
  Serial.print(dwy, DEC);
  Serial.print("\t");
  Serial.print(dwz, DEC);
  Serial.print("\t");

  upSignal+=tilt();
  count++;
  upSignal=stint(upSignal);
  delay(1);
  if(count%30==0)emit(link(upSignal,dwy,dwx,dwz));
  
  //MadgwickAHRSupdate(gx,gy,gz,ax,ay,az,mx,my,mz);
  
//  float R11 = 2.*q0*q0 -1 +2.*q1*q1;
//  float R21 = 2.*(q1*q2 - q0*q3);
//  float R31 = 2.*(q1*q3 + q0*q2);
//  float R32 = 2.*(q2*q3 - q0*q1);
//  float R33 = 2.*q0*q0 -1 +2.*q3*q3;
//  
//  float phi   =  atan2(R32, R33 );
//  float theta =  -atan(R31 / sqrt(1-R31*R31) );
//  float psi   =  atan2(R21, R11 );
//
//  phi   = phi*180.0/M_PI;
//  theta = theta*180.0/M_PI;
//  psi   = psi*180.0/M_PI;
//
//  q_phi.push_back(phi);
//  q_theta.push_back(theta);
//  q_psi.push_back(psi);
  
//  Serial.print(phi);
//  Serial.print("\t");
//  Serial.print(theta);
//  Serial.print("\t");
//  Serial.print(psi);
//  Serial.print("\t");

  // direction
  
  // End of line
  Serial.println("");
//  delay(100);    
}


//---------------------------------------------------------------------------------------------------
// Fast inverse square-root
// See: http://en.wikipedia.org/wiki/Fast_inverse_square_root

float invSqrt(float x) {
  float halfx = 0.5f * x;
  float y = x;
  long i = *(long*)&y;
  i = 0x5f3759df - (i>>1);
  y = *(float*)&i;
  y = y * (1.5f - (halfx * y * y));
  return y;
}

int tilt(){
  int nowTS1=0, nowTS2=0;
  nowTS1=digitalRead(tiltInput1);
  nowTS2=digitalRead(tiltInput2);
  if(nowTS1==nowTS2)nowTS=nowTS1;
  if(TS!=nowTS){
    TS=nowTS;
    if(count>=100){
      count=0;
      return 2;
    } else {
      return 0;
    }
  }
  else if(count>=300){
    count=0;
    return -10;
  }
  else return 0;
}
int stint(int num){
  if(num>=20)return 20;
  else if(num<=0)return 0;
  else return num;
}
long link(long num1,long num2,long num3,long num4) {
  return num1*1000000+num2*10000+num3*100+num4;
}
void emit(long data) {
  Serial.print("Sending:");
  Serial.print(data);
  bool ok=radio.write(&data,sizeof(long));
  if(ok)
    Serial.println(".....successed");
  else
    Serial.println(".....failed");
}
